#include "control.h"

#include "C_code_Log.h"
#include "motor.h"
#include "string.h"

static runStateTypeDef gl_runState;              // 全局路段运行状态记录器
ControlCenterTypeDef gl_controlCenter;    // 全局控制中心
extern const uint8_t runMethodTable[][100];  // 路线图，主函数中定义了

/// @brief 路段运行状态初始化
/// @param runState 路段运行状态记录器
static void runStateInit(runStateTypeDef *runState) {
  runState->F_LineState = MY_READY;         // 就绪，需要执行
  runState->F_RoadBlockState = MY_BLOCKED;  // 阻塞，等待加速状态来触发
  runState->speedUpState = MY_READY;        // 就绪，需要执行
  runState->seekNodeState = MY_BLOCKED;  // 阻塞，等待第二次加速状态来触发
  runState->carParkState = MY_BLOCKED;  // 阻塞，等待找点状态来触发
  runState->RotateState = MY_BLOCKED;   // 阻塞，等待停车状态来触发
}



static void linkInfoUpdate(ControlCenterTypeDef *controlCenter,
                           const linkInfoTypeDef mapInfo[][NODE_FORK_NUM]);



/// @brief 控制中心初始化
/// @param controlCenter 控制台
/// @param mapInfo 地图信息
/// @param runMethodTable 一条路线图
void controlCenterInit(ControlCenterTypeDef *controlCenter,
                       const linkInfoTypeDef mapInfo[][NODE_FORK_NUM],
                       const uint8_t *runMethodTable) {
  if (strlen((const char *)runMethodTable) < 3) {
    LOG("ERROR", "路线图长度不够");
    motorStop();
    while (1)
      ;
  } else {
    controlCenter->nodeNum = 0;//已走的节点数，因为现在一个点都没走过，所以为0
    controlCenter->runState = &gl_runState;//路段运行状态记录器赋初值
    controlCenter->update = MY_READY;//更新状态赋初值
    controlCenter->runMethodTable = runMethodTable;//更新状态赋初值 新增2024/1/12

    linkInfoUpdate(controlCenter, mapInfo);//路段信息更新
    LOG("INFO", "下一节点为%d,下下节点为%d",
        controlCenter->sectionInfo->nextNode,
        controlCenter->sectionInfo->nextNextNode);
    LOG("INFO", "控制中心初始化完成");
  }
}

/// @brief 控制中心更新
/// @param controlCenter 控制台
/// @param mapInfo 地图信息
/// @param runMethodTable 一条路线图
/// @note  路段走完时调用此函数，会更新控制台的信息，
/// 包括更新路段运行状态，把已走的节点数加1（标记自己已经走到了这个节点），更新控制台中的路段信息
void controlCenterUpdate(ControlCenterTypeDef *controlCenter,
                         const linkInfoTypeDef mapInfo[][NODE_FORK_NUM]) {
  // 更新路段运行状态
  runStateInit(controlCenter->runState);
  // 节点数加1
  controlCenter->nodeNum++;
  // 更新路段信息
  linkInfoUpdate(controlCenter, mapInfo);
  LOG("INFO", "下一节点为%d,下下节点为%d", controlCenter->sectionInfo->nextNode,
      controlCenter->sectionInfo->nextNextNode);
  LOG("INFO", "控制中心更新完成");
  // 巡线方法初始化标志位清零
  controlCenter->findLineFlag = 0;
  // 巡线方法等于路段信息中的巡线方法
  controlCenter->findLineWay = controlCenter->sectionInfo->findLineWay;
}

/// @brief 路段信息更新
/// @param controlCenter 控制台
/// @param mapInfo 地图信息
/// @note 输入控制台，会根据控制台中记录走过的节点数和路线图决定当前节点，下个节点，下下个节点，
/// 在地图信息中找到对应的路段信息，更新进控制台，同时更新控制台的状态为就绪态，
/// 如果已经走到路线的倒数第二个节点，则更新为退出态
static void linkInfoUpdate(ControlCenterTypeDef *controlCenter,
                           const linkInfoTypeDef mapInfo[][NODE_FORK_NUM]) {
  // 走过的节点数
  uint8_t currentNum = controlCenter->nodeNum;
  uint8_t nextNum = currentNum + 1;         
  //指向当前路段信息的指针，比如现在在节点1，则先指向mapInfo[1][]的第一个元素
  const linkInfoTypeDef *sectionInfo = &mapInfo[controlCenter->runMethodTable[currentNum]][0];

  //确保更新为就绪态，即要进行路线更新
  if (controlCenter->update == MY_READY) {
  // 如果当前走过的节点数等于路线图的长度-2，说明已经走到路线的倒数第二个节点，更新状态置为退出
    if (currentNum >= strlen((const char *)controlCenter->runMethodTable) - 2) {
      controlCenter->update = MY_EXIT;
      return;
    }
    //根据路线图中的下一个节点和下下个节点，在地图中找到对应的路段信息
    for (int i = 0; i < NODE_FORK_NUM; i++) {
      if (sectionInfo[i].nextNode == controlCenter->runMethodTable[nextNum]) {
        if (sectionInfo[i].nextNextNode == controlCenter->runMethodTable[nextNum + 1]) {
          controlCenter->sectionInfo = &sectionInfo[i];
          controlCenter->update = MY_Running;
          return;
        }
      }
    }
    //没有找到的处理
    LOG("ERROR", "没有找到对应的路段信息");
    motorStop();
    while (1)
      ;
  }
}

/// @brief 切换路线
/// @param controlCenter 控制台
/// @param mapInfo 地图信息
/// @param runMethodTable 要切换的路线图
/// @note 重新初始化一下控制台就好了
void changeRunMethodTable(ControlCenterTypeDef *controlCenter,
                          const linkInfoTypeDef mapInfo[][NODE_FORK_NUM],
                          const uint8_t *runMethodTable) {
    controlCenterInit(controlCenter, mapInfo, runMethodTable);
}